#!/usr/bin/python3
# -*- coding: utf-8 -*-
import EventLog as log
import time,sys

import D_DQ4 as D
import numpy as np
import math

import matplotlib.pyplot as plt

defauleJDD = {11: 0, 12: 0, 13: 0, 14: 10, 15: 10, 16: 0
              , 21: 0, 22: 0, 23: 0, 24: 10, 25: 10, 26: 0
              , 31: 10, 32: 10, 33: 0, 34: 20, 35: 0, 36: 0, 37: 0
              , 41: 10, 42: 10, 43: 0, 44: 20, 45: 0, 46: 0, 47: 0
              , 71: 0, 72: 0}

class PoseFlow:
    def __init__(self):
        self.L1 = D.LinkDQ40()
        # readJDDlist = [11,12,13,14,21,22,23,24,31,32,41,42,71,72]
        readJDDlist = [11,12,13,14,21,22,23,24]
        # readJDDlist = []
        for i in range(10):
            ret,rlt = self.L1.GetJointData(readJDDlist)
            print("第一次读取电机数据：",ret)
            if sum(ret) == len(ret):break
        self.readJDDret = dict(zip(readJDDlist, ret))     # 电机ID -> 是否读取成功
        self.readJDDrlt = dict(zip(readJDDlist, rlt))     # 电机ID -> 是否读取成功
        self.inJDD = defauleJDD                           # 所有电机角度
        self.inJDD.update(dict(zip(readJDDlist, rlt)))    # 部分电机通过读取机器人关节角度来设置初始角度
        
        if sum(ret)==len(ret):
            log.logger.info("成功读取全部电机角度：%s"%self.readJDDrlt)
        else:
            log.logger.error("无法读取全部的电机角度：%s"%self.readJDDret)
            sys.exit()

        # self.lasttime = time.time()
    def _InterpolationSin(self,TP,step=1,sd = [-math.pi/2,math.pi/2]):
        # sd表示sin函数取值段，范围在-1.57到1.57之间，中间值为0
        if sd[0]<-math.pi/2:sd[0] = -math.pi/2
        if sd[1]>math.pi/2:sd[1] = math.pi/2
        if step<=2:
            return TP
        else:
            rlist = np.zeros((step,len(TP[0])))
            rlist1 = np.zeros((step, len(TP[0])))
            for i in range(len(TP[0])):
                aline = np.linspace(TP[0][i],TP[1][i],step)
                jline = np.linspace(sd[0],sd[1],step)
                for j in range(len(aline)):
                    rlist[j][i] = aline[j]
                    # sq = (math.sin(jline[j])+1)/2
                    sq = (math.sin(jline[j])-math.sin(sd[0]))/(math.sin(sd[1])-math.sin(sd[0]))
                    rlist1[j][i] = round((TP[1][i]-TP[0][i])*sq+TP[0][i],2)
            return rlist1

    def SetPoseFlowDic_simple(self,JDD, sleeptime=0.5, steps=5):
        # print("JDD",JDD)
        JointsDataDic = JDD.copy()
        JointsDataDicStart = self.inJDD  # ! 注意，这里的inJDD是全部的电机角度
        JointsNameList = list(JointsDataDic.keys())
        endposedata = []
        startpostdata = []

        for key in JointsDataDic.keys():  # ! 在这里，按照输入电机JDD，从全部电机初始
            endposedata.append(JointsDataDic[key])
            startpostdata.append(JointsDataDicStart[key])

        TP = [startpostdata, endposedata]
        setppostdatas = self._InterpolationSin(TP, steps)

        for i,v in enumerate(setppostdatas):
            everypose = JDD.copy()
            everypose.update(dict(zip(JointsNameList, v)))
            self.L1.SetJointData(JointsNameList, v)
            time.sleep(sleeptime)
            # print("sleeptime",sleeptime,everypose)

        # ! 注意，这里的inJDD是全部的电机角度
        # JDD和inJDD的电机的维度并不相同，使用JDD把inJDD中变化的角度更新掉
        self.inJDD.update(JDD)
    
    def SetPoseFlowDic(self,JDD, JDDcut = {},sleeptime=0.5, steps=5):
        # JDDcut用于自定义只是用部分sin函数进行插值的关节电机
        # 格式是JDDcut={13:[-2,0]},字典的key代表函数的id,字典的value代表截取函数的线段，值为[-0.5*pi,0.5*pi],超出部分自动截取掉
        
        JointsDataDic = JDD.copy()
        JointsDataDicStart = self.inJDD  # ! 注意，这里的inJDD是全部的电机角度
        JointsNameList = list(JointsDataDic.keys())
        
        allpose = np.empty((steps,len(JDD)))
        
        n = 0
        for key in JointsDataDic.keys():  # ! 在这里，按照输入电机JDD，从全部电机初始
            TP = [[JointsDataDicStart[key]],[JointsDataDic[key]]]
            # key在JDDcut中的采用部分sin函数，默认采用完成sin函数
            if key in JDDcut.keys():
                everystep = self._InterpolationSin(TP, steps,JDDcut[key])[:,0]
            else:
                everystep = self._InterpolationSin(TP, steps)[:,0]
            for j,v in enumerate(everystep):
                allpose[j,n] = v
            
            n = n + 1
        
        for i,v in enumerate(allpose):
            if i == 0:continue
            everypose = JDD.copy()
            everypose.update(dict(zip(JointsNameList, v)))
            self.L1.SetJointData(JointsNameList, v)
            time.sleep(sleeptime)
            # print("sleeptime",sleeptime,everypose[13],everypose[23])
            # nowtime =  time.time()
            # log.logger.warning("%s,%s,%s,%s,%s"%(nowtime-self.lasttime,everypose[13],everypose[23],everypose[14],everypose[24]))
            # self.lasttime = nowtime
        # ! 注意，这里的inJDD是全部的电机角度
        # JDD和inJDD的电机的维度并不相同，使用JDD把inJDD中变化的角度更新掉
        self.inJDD.update(JDD)

    def __del__(self):
        self.L1.__del__()

if __name__ == "__main__":
    P = PoseFlow()
    
    P.SetPoseFlowDic({13:0,14:40}, sleeptime=0.1, steps=30)
    time.sleep(5)
    
    P.SetPoseFlowDic({13:20,14:60},JDDcut={13:[-2,0]}, sleeptime=0.05, steps=10)
    
    P.SetPoseFlowDic({13:35,14:40},JDDcut={13:[0,2]}, sleeptime=0.05, steps=10)
    time.sleep(2)
    
    # P.SetPoseFlowDic({34:45,44:45}, sleeptime=0.1, steps=20)
    # time.sleep(2)

    # JDD = {11: 0, 12: 0, 13: 15, 14: 40, 15: 25, 16: 0, 21: 0, 22: 0, 23:15, 24: 40, 25: 25, 26: 0, 71: 0, 72: 0}
    # P.SetPoseFlowDic(JDD, sleeptime=0.1, steps=5)
    # while True:
    #     JDD = {11: 0, 12: 0, 13: 15, 14: 40, 15: 25, 16: 10, 21: 0, 22: 0, 23:15, 24: 40, 25: 25, 26: 10}
    #     P.SetPoseFlowDic(JDD, sleeptime=0.1, steps=5)
    #     JDD = {11: 0, 12: 0, 13: 5, 14: 20, 15: 15, 16: -10, 21: 0, 22: 0, 23:5, 24: 20, 25: 15, 26: -10}
    #     P.SetPoseFlowDic(JDD, sleeptime=0.1, steps=5)

    # 电机伸缩同步性测试
    # for i in range(5):
    #     # time.sleep(1)
    #     JDD = {11: 0, 12: 0, 13: 15, 14: 40, 15: 25, 16: 13, 21: 0, 22: 0, 23:15, 24: 40, 25: 25, 26: 13}
    #     P.SetPoseFlowDic(JDD, sleeptime=0.1, steps=5)
    #     # time.sleep(1)
    #     JDD = {11: 0, 12: 0, 13: 0, 14: 10, 15: 10, 16: 0, 21: 0, 22: 0, 23: 0, 24: 10, 25: 10, 26: 0}
    #     P.SetPoseFlowDic(JDD, sleeptime=0.1, steps=5)

    # 迈腿测试
    # for i in range(5):
    #     JDD = {11: 0, 12: 0, 13: 35, 14: 50, 15: 5, 16: 0, 21: 0, 22: 0, 23: 15, 24: 40, 25: 25, 26: 0}
    #     P.SetPoseFlowDic(JDD, sleeptime=0.1, steps=7)
    #     JDD = {11: 0, 12: 0, 13: 15, 14: 40, 15: 25, 16: 0, 21: 0, 22: 0, 23: 35, 24: 50, 25: 5, 26: 0}
    #     P.SetPoseFlowDic(JDD, sleeptime=0.1, steps=7)
        
    # 金鸡独立
    # JDD = {11: 0, 12: 0, 13: 15, 14: 40, 15: 25, 16: 0, 21: 0, 22: 0, 23: 15, 24: 40, 25: 25, 26: 0}
    # P.SetPoseFlowDic(JDD, sleeptime=0.1, steps=10)
    # print("---------------------")
    # JDD = {11: 0, 12: 10, 13: 25, 14: 60, 15: 30, 16: -10, 21: 0, 22: -10, 23: 5, 24: 10, 25: 10, 26: 10}
    # P.SetPoseFlowDic(JDD, sleeptime=0.1, steps=10)
    
    # time.time(10)
    # JDD = {11: 0, 12: 10, 13: 25, 14: 30, 15: 10, 16: -10, 21: 0, 22: -10, 23: 5, 24: 10, 25: 10, 26: 10}
    # P.SetPoseFlowDic(JDD, sleeptime=0.1, steps=7)

    del P
    # 迈腿惯性测试

